using System;
using System.Collections.Generic;
using System.Data.Linq;
using System.Text;

namespace GraphSpace
{
    public class BiSearch<PQ1, PQ2>
        where PQ1 : IPQ<BiDijkstraKnoten>, new()
        where PQ2 : IPQ<BiDijkstraKnoten>, new()
	{
		private PQ1 PriorityStuct1 = new PQ1 ();
		private PQ2 PriorityStuct2 = new PQ2 ();

        public BiSearch(int C)
        {
            PriorityStuct1.init(C);
            PriorityStuct2.init(C);
        }

        public BiSearch()
        {

        }

        public double DoSearch(Graph<BiDijkstraKnoten, Kante> G1, Graph<BiDijkstraKnoten, Kante> G2, int initialKnoten, int endKnoten, out Knoten[] Path, bool GoalDirected1, bool GoalDirected2)
        {
            initPQ1(G1);
            initPQ2(G2);

            Knoten aktKnoten;

            while (true)
            {
                if (DoSearchStep1(G1,G2, G1.V[initialKnoten], G1.V[endKnoten], GoalDirected1, out aktKnoten))
                    break;

                if (DoSearchStep2(G2,G1, G2.V[endKnoten], G2.V[initialKnoten], GoalDirected2, out aktKnoten))
                    break;
            }

            List<Knoten> pathlist = new List<Knoten>();
            BiDijkstraKnoten pred = G1.V[aktKnoten.Id];
            while (pred.predeseccor != null)
            {
                pathlist.Add(pred);
                pred = (BiDijkstraKnoten)pred.predeseccor;
            }
            pathlist.Reverse();
            pred = (BiDijkstraKnoten)G2.V[aktKnoten.Id];
            if(pred != null)
            while (pred.predeseccor != null)
            {
                pathlist.Add(pred);
                pred = (BiDijkstraKnoten)pred.predeseccor;
            }
            
            Path = pathlist.ToArray();

            if (GoalDirected1 && GoalDirected2)
                return G1.V[aktKnoten.Id].realdist + G2.V[aktKnoten.Id].realdist;
            if (GoalDirected1 && !GoalDirected2)
                return G1.V[aktKnoten.Id].realdist + G2.V[aktKnoten.Id].distance;
            if (!GoalDirected1 && GoalDirected2)
                return G1.V[aktKnoten.Id].distance + G2.V[aktKnoten.Id].realdist;
            return G1.V[aktKnoten.Id].distance + G2.V[aktKnoten.Id].distance;
        }

        bool DoSearchStep1(Graph<BiDijkstraKnoten, Kante> G, Graph<BiDijkstraKnoten, Kante> G2, Knoten initialKnoten, Knoten endKnoten, bool GoalDirected, out Knoten AktKnoten)
        {
            bool foundBySearch = false;
            AktKnoten = null;
            try
            {
                ((BiDijkstraKnoten)initialKnoten).distance = 0;
                ((BiDijkstraKnoten)initialKnoten).foundBySearch = true;
               // while (!PriorityStuct1.isEmpty())
               // {

                    Knoten aktKnoten = PriorityStuct1.extractHead();
                    if (((BiDijkstraKnoten)aktKnoten).foundBySearch)
                    {
                        foundBySearch = true;
                        AktKnoten = aktKnoten;
                        ((BiDijkstraKnoten)G2.V[aktKnoten.Id]).foundBySearch = true;
                    }
                    ((BiDijkstraKnoten)aktKnoten).foundBySearch = true;
                    //if (aktKnoten.Id == endKnoten.Id)
                    //    break;
                    ((BiDijkstraKnoten)aktKnoten).besucht = true;
                    for (int i = 0; i < G.E[aktKnoten.Id].Count; i++)
                    {
                        Knoten ZielKnoten = (Knoten)G.E[aktKnoten.Id][i].Vnach;
                        if (!((BiDijkstraKnoten)ZielKnoten).besucht)
                        {
                            //Unterschied zwischen normal oder zielgerichtet
                            if (GoalDirected)
                            {
                                // neue abstandsberechnung fuer AStar
                                //int NewDist = aktKnoten.distance + (int)G.E[aktKnoten.Id][i].Gewicht;

                                int AbstandX = ((BiDijkstraKnoten)endKnoten).x - ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).x;
                                int AbstandY = ((BiDijkstraKnoten)endKnoten).y - ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).y;
                                double NewDist = ((BiDijkstraKnoten)aktKnoten).distance + G.E[aktKnoten.Id][i].Gewicht + Math.Sqrt(AbstandX * AbstandX + AbstandY * AbstandY);
                                if (NewDist < ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance)
                                {
                                    ((BiDijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
                                    PriorityStuct1.decreaseKey(((BiDijkstraKnoten)ZielKnoten).getHeapPos(), NewDist);
                                    ((BiDijkstraKnoten)ZielKnoten).realdist = ((BiDijkstraKnoten)aktKnoten).realdist + G.E[aktKnoten.Id][i].Gewicht;
                                }
                            }
                            else
                            {
                                if (!((BiDijkstraKnoten)ZielKnoten).besucht)
                                {
                                    double NewDist = ((BiDijkstraKnoten)aktKnoten).distance + (int)G.E[aktKnoten.Id][i].Gewicht;
                                    if (NewDist < ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance)
                                    {
                                        ((BiDijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
                                        PriorityStuct1.decreaseKey(((BiDijkstraKnoten)ZielKnoten).getHeapPos(), NewDist);
                                    }
                                }
                            }
                        }
                    }
              //  }


            }
            catch (Exception e)
            {
                Console.WriteLine(e.Message);

            }
            return foundBySearch;

        }

        private void initPQ1(Graph<BiDijkstraKnoten, Kante> G)
        {
            for (int i = 0; i < G.V.Count; i++)
            {
                try
                {
                    PriorityStuct1.insert((BiDijkstraKnoten)G.V[i]);
                }
                catch (Exception e)
                {
                    Console.WriteLine(e.Message);
                }
            }
        }

        bool DoSearchStep2(Graph<BiDijkstraKnoten, Kante> G, Graph<BiDijkstraKnoten, Kante> G2, Knoten initialKnoten, Knoten endKnoten, bool GoalDirected, out Knoten AktKnoten)
        {
            bool foundBySearch = false;
            AktKnoten = null;
            try
            {
                ((BiDijkstraKnoten)initialKnoten).distance = 0;
                ((BiDijkstraKnoten)initialKnoten).foundBySearch = true;
                // while (!PriorityStuct2.isEmpty())
                // {

                Knoten aktKnoten = PriorityStuct2.extractHead();
                if (((BiDijkstraKnoten)aktKnoten).foundBySearch)
                {
                    foundBySearch = true;
                    AktKnoten = aktKnoten;
                    ((BiDijkstraKnoten)G2.V[aktKnoten.Id]).foundBySearch = true;
                }
                //    if (aktKnoten.Id == endKnoten.Id)
                //        break;
                ((BiDijkstraKnoten)aktKnoten).besucht = true;
                for (int i = 0; i < G.E[aktKnoten.Id].Count; i++)
                {
                    Knoten ZielKnoten = (Knoten)G.E[aktKnoten.Id][i].Vnach;
                    if (!((BiDijkstraKnoten)ZielKnoten).besucht)
                    {
                        //Unterschied zwischen normal oder zielgerichtet
                        if (GoalDirected)
                        {
                            // neue abstandsberechnung fuer AStar
                            //int NewDist = aktKnoten.distance + (int)G.E[aktKnoten.Id][i].Gewicht;

                            int AbstandX = ((BiDijkstraKnoten)endKnoten).x - ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).x;
                            int AbstandY = ((BiDijkstraKnoten)endKnoten).y - ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).y;
                            double NewDist = ((BiDijkstraKnoten)aktKnoten).distance + G.E[aktKnoten.Id][i].Gewicht + Math.Sqrt(AbstandX * AbstandX + AbstandY * AbstandY);
                            if (NewDist < ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance)
                            {
                                ((BiDijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
                                PriorityStuct2.decreaseKey(((BiDijkstraKnoten)ZielKnoten).getHeapPos(), NewDist);
                                ((BiDijkstraKnoten)ZielKnoten).realdist = ((BiDijkstraKnoten)aktKnoten).realdist + G.E[aktKnoten.Id][i].Gewicht;
                            }
                        }
                        else
                        {
                            if (!((BiDijkstraKnoten)ZielKnoten).besucht)
                            {
                                double NewDist = ((BiDijkstraKnoten)aktKnoten).distance + (int)G.E[aktKnoten.Id][i].Gewicht;
                                if (NewDist < ((BiDijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance)
                                {
                                    ((BiDijkstraKnoten)ZielKnoten).predeseccor = aktKnoten;
                                    PriorityStuct2.decreaseKey(((BiDijkstraKnoten)ZielKnoten).getHeapPos(), NewDist);
                                }
                            }
                        }
                    }
                }

                //}

            }
            catch (Exception e)
            {
                Console.WriteLine(e.Message);

            }
            return foundBySearch;
        }

        private void initPQ2(Graph<BiDijkstraKnoten, Kante> G)
        {
            for (int i = 0; i < G.V.Count; i++)
            {
                try
                {
                    PriorityStuct2.insert((BiDijkstraKnoten)G.V[i]);
                }
                catch (Exception e)
                {
                    Console.WriteLine(e.Message);
                }
            }
        }
	}
}

